#include "uav_swarms/Follower.hpp"
// 订阅器回调函数，用于接收编队模式数据
    void Follower::formation_pattern_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
        // 将数据转化为Eigen矩阵形式，并存储
        Eigen::VectorXf data = Eigen::Map<const Eigen::VectorXf>(msg->data.data(), msg->data.size());
        formation_pattern_ = Eigen::Map<const Eigen::MatrixXf>(data.data(), 3, uav_num);
        // std::cout << formation_pattern_ << std::endl;
        RCLCPP_INFO(this->get_logger(),"received values");
    }

    // 订阅器回调函数，更新无人机的当前位姿
    void Follower::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        pose_ = *msg;
    }

    // 订阅器回调函数，接收领队的位姿信息
    void Follower::leader_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        leader_pose_ = *msg;
    }

    // 订阅器回调函数，接收避障速度数据
    void Follower::avoid_vel_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
        avoid_vel_ = *msg;
    }

    void Follower::leader_vel_callback(const  geometry_msgs::msg::TwistStamped::SharedPtr msg){
        std::lock_guard<std::mutex> lock_guard(leader_velocity_mutex);
        leader_velocity = *msg;
    }
    void Follower::follower_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg){
        std::lock_guard<std::mutex> lock_guard(follower_velocity_mutex);
        follower_velocity = *msg;
    }
    
    // 主控制循环，用于计算并发布速度指令
    void Follower::control_loop() {

        // 等待一段时间，确保所有无人机都订阅了编队模式
        if (service_status == false)
        {
            RCLCPP_INFO(this->get_logger(), "Waiting for formation pattern...");
            return;
        }
        

        // 只有在获取到编队模式后才进行控制
        if (formation_pattern_.size() != 0) {
            geometry_msgs::msg::Twist cmd_vel_enu;

            // 计算leader和follower之间的相对位置，并计算速度指令
            double p_diff_x = leader_pose_.pose.position.x + formation_pattern_(0, uav_id_)- pose_.pose.position.x;
            double p_diff_y = leader_pose_.pose.position.y + formation_pattern_(1, uav_id_)- pose_.pose.position.y;
            double p_diff_z = leader_pose_.pose.position.z + formation_pattern_(2, uav_id_)- pose_.pose.position.z;

            // double p_diff_x_v = p_diff_x / delta_t_ - follower_velocity.twist.linear.x;
            // double p_diff_y_v = p_diff_y / delta_t_ - follower_velocity.twist.linear.y;
            // double p_diff_z_v = p_diff_z / delta_t_ - follower_velocity.twist.linear.z;
            // 计算与领队的速度差异
            double v_diff_x = leader_velocity.twist.linear.x - follower_velocity.twist.linear.x;
            double v_diff_y = leader_velocity.twist.linear.y - follower_velocity.twist.linear.y;
            double v_diff_z = leader_velocity.twist.linear.z - follower_velocity.twist.linear.z;

            // 计算与领队的距离，并通过比例控制计算速度指令
            cmd_vel_enu.linear.x = p_diff_x * Kp_p * delta_t_ * diffusion_positon + v_diff_x * Kp_v * delta_t_ *diffusion_velocity;
            cmd_vel_enu.linear.y = p_diff_y * Kp_p * delta_t_* diffusion_positon + v_diff_y * Kp_v * delta_t_ *diffusion_velocity;
            cmd_vel_enu.linear.z = p_diff_z * Kp_p * delta_t_* diffusion_positon + v_diff_z * Kp_v * delta_t_ *diffusion_velocity;

            // cmd_vel_enu.linear.x = Kp_ * ((leader_pose_.pose.position.x + formation_pattern_(0, uav_id_)) - pose_.pose.position.x);
            // cmd_vel_enu.linear.y = Kp_ * ((leader_pose_.pose.position.y + formation_pattern_(1, uav_id_)) - pose_.pose.position.y);
            // cmd_vel_enu.linear.z = Kp_ * ((leader_pose_.pose.position.z + formation_pattern_(2, uav_id_)) - pose_.pose.position.z);
            // 添加避障速度
            cmd_vel_enu.linear.x += Kp_avoid_ * avoid_vel_.x * delta_t_ *diffusion_avoid;
            cmd_vel_enu.linear.y += Kp_avoid_ * avoid_vel_.y * delta_t_ *diffusion_avoid;
            cmd_vel_enu.linear.z += Kp_avoid_ * avoid_vel_.z * delta_t_ *diffusion_avoid;
            

            // 限制速度的最大值
            double cmd_vel_magnitude = sqrt(pow(cmd_vel_enu.linear.x, 2) + pow(cmd_vel_enu.linear.y, 2) + pow(cmd_vel_enu.linear.z, 2));
            if (cmd_vel_magnitude > sqrt(3) * vel_max_) {
                cmd_vel_enu.linear.x = cmd_vel_enu.linear.x / cmd_vel_magnitude * vel_max_;
                cmd_vel_enu.linear.y = cmd_vel_enu.linear.y / cmd_vel_magnitude * vel_max_;
                cmd_vel_enu.linear.z = cmd_vel_enu.linear.z / cmd_vel_magnitude * vel_max_;
            }

            // // 如果速度变化超过阈值，计算无人机的航向角（yaw），并进行角速度控制
            // if (fabs(cmd_vel_enu.linear.y) > 5.0 || fabs(cmd_vel_enu.linear.x) > 3.0) {
            //     double qx = pose_.pose.orientation.x;
            //     double qy = pose_.pose.orientation.y;
            //     double qz = pose_.pose.orientation.z;
            //     double qw = pose_.pose.orientation.w;

            //     // 计算领队的yaw角
            //     double yaw_leader = atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));
            //     cmd_vel_enu.angular.z = (atan2(cmd_vel_enu.linear.y, cmd_vel_enu.linear.x) - yaw_leader) * 4 * 0.05;
            // } else {
            //     cmd_vel_enu.angular.z = 0.0;
            // }

            // 发布速度指令
            vel_enu_pub_->publish(cmd_vel_enu);
            
            RCLCPP_INFO(this->get_logger(),"published twist is: %f, %f, %f", cmd_vel_enu.linear.x, cmd_vel_enu.linear.y, cmd_vel_enu.angular.z);
            // RCLCPP_INFO(this->get_logger(),"published twist is: %f, %f, %f", formation_pattern_(0, uav_id_), formation_pattern_(1, uav_id_), formation_pattern_(2, uav_id_));
        }
    }


void Follower::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
    bool status = msg->status;
    {
        std::lock_guard<std::mutex> lock(server_status_mutex_);
        service_status = status;
    }
    RCLCPP_INFO(this->get_logger(), "Service status changed to %d", service_status);
}